from math import pi, sqrt
from typing import List

import numpy.linalg as lg

from User.Releases.RRT_local_minimum import RRT
from Modules.foward_kinematics import aubo_FK
from Modules.auboi5_collision_check import auboi5_collision_check
from Modules.get_distance import *
from Modules.robot_class import *

from multiprocessing import Process, Queue

import time

def get_obstacle(skeleton_position: List, joint_positiopn: List) -> Cube_obstacle:
    """
    * This is an interface to get the information of obstacle
    * 筛选出最有可能发生碰撞的骨骼坐标
    """
    step = 3
    distance = list()
    skeletons = list()
    for j in range(0, len(joint_positiopn), step):
        for i in range(0, len(skeleton_position), step):
            skeleton = Point(skeleton_position[i:i + step])
            joint = Point(joint_positiopn[j:j + step])
            # distance.append(lg.norm(joint.vector - skeleton.vector))
            distance.append(distance_point_to_point(joint, skeleton))
            skeletons.append(list(skeleton.p))
            # print("distance", distance)
    min_index = distance.index(min(distance))
    # print(distance[min_index])
    # index = distance.index(min_distance)
    # print(min_distance)
    # print(index)
    # print("distance",distance)
    # print("distance size: ",len(distance))
    # print("skeletons",skeletons)
    # print("skeletons size: ",len(skeletons))
    # print("min_index",min_index)

    # 确定距离机器人关节最近的骨骼点坐标
    min_distance_skeleton_to_joint = skeletons[min_index]
    # print(skeleton_min_distance)

    # 将骨骼点坐标实例化为实体障碍物
    cube_obstacle = Cube_obstacle(Point(min_distance_skeleton_to_joint), 0.2, 0.2, 0.2)
    # print("obstacle positiopn(xyz,lwh):",cube_obstacle.x,cube_obstacle.y,cube_obstacle.z,cube_obstacle.l,cube_obstacle.w,cube_obstacle.h)
    return cube_obstacle


class APF_RRT:
    """
    Artificial Poential Field and RRT
    """

    def __init__(self, start: Joint_point, goal: Joint_point, cube_obstacle: Cube_obstacle,
                 joint_limit_angle: List[Joint_point]) -> None:
        self.start = start  # 机器人关节角起点
        self.goal = goal  # 机器人关节角终点
        self.cube_obstacle = cube_obstacle  # 障碍物
        self.joint_limit_angle = joint_limit_angle  # 机器人关节限制角度,一般正反180度

        # Get the goal end point in Cartesian space
        T = aubo_FK(self.goal)
        self.goal_point = Point([T[5][0, 3], T[5][1, 3], T[5][2, 3]])  # 末端位置
        # print("末端位置:",self.goal_point.p)    # 没问题

        # Set the Katt and Krep
        self.Katt_end = 0.5
        self.Katt_joint = 0.1
        self.Krep = 0.1

        # Set the range of repulsive potential field,设置排斥电位场的范围
        self.d0 = 0.3

        # Set the step length，步长，默认2°
        self.step_length = 20 * pi / 180

        # Set a signal to record whether stuck in local minimum，陷入局部最小值标志
        self.stuck_in_local_minimum = False

        # 桌子建模需要
        self.limit_position = float(0.2)

    def get_attractive_energy(self, joint_point: Joint_point) -> float:
        """
        * Get the attractive energy in joint_point
        """
        if not self.stuck_in_local_minimum:
            # Set the self.goal as the attractive joint_point
            # Get the endlink position of theta
            T = aubo_FK(joint_point)
            endlink_point = Point([T[5][0, 3], T[5][1, 3], T[5][2, 3]])

            # Get the distance between endlink and goal
            d = distance_point_to_point(endlink_point, self.goal_point)

            # Return the attractive energy
            return 0.5 * self.Katt_end * (d ** 2) + 0.5 * self.Katt_joint * lg.norm(
                self.goal.vector - joint_point.vector) ** 2
        else:
            # Set the self.new_goal as the attractive joint_point
            # Get the endlink position of theta
            T = aubo_FK(joint_point)
            endlink_point = Point([T[5][0, 3], T[5][1, 3], T[5][2, 3]])

            # Get the distance between endlink and new_goal
            d = distance_point_to_point(endlink_point, self.new_goal_point)

            # Return the attractive energy
            return 0.5 * self.Katt_end * (d ** 2) + 0.5 * self.Katt_joint * lg.norm(
                self.new_goal.vector - joint_point.vector) ** 2

    def get_repulsive_energy(self, joint_point: Joint_point) -> float:
        """
        * Get the repulsive energy in joint_point
        """
        # Get the repulsive energy of each DH joints
        Ui = list()  # Stores the repulsive energy of each DH joints
        T = aubo_FK(joint_point)
        for A in T:
            DH_joint_point = Point([A[0, 3], A[1, 3], A[2, 3]])
            # print("DH_joint_point:",DH_joint_point.p)
            d = distance_point_to_cube_obstacle(DH_joint_point,
                                                self.get_obstacle())
            if d < self.d0:
                Ui.append(0.5 * self.Krep * (1 / d - 1 / self.d0))
            else:
                Ui.append(0)

        return sum(Ui)

    def get_obstacle(self) -> Cube_obstacle:
        """
        * This is an interface to get the information of obstacle
        """

        return self.cube_obstacle

    def deal_with_local_minimum(self, start:Joint_point,joint_point: Joint_point) -> None:
        """
        Method to deal with algorithms getting stuck in local minimum
        """
        """
        * Use RRT to generate a new joint_point as a attractive joint point
        """
        # Get a new attractive joint_point
        # rrt = RRT(start=joint_point, goal=self.goal, cube_obstacle=self.get_obstacle(),
        #           joint_limit_angle=self.joint_limit_angle, num_iterations=100)
        rrt = RRT(start=start, goal=self.goal, cube_obstacle=self.get_obstacle(),
                  joint_limit_angle=self.joint_limit_angle, num_iterations=100)
        self.new_goal = rrt.path_planning()[-1]  # 返回path路径点(21个，从star到临时目标点)，只取最后一个作为临时目标点

        # Get the new goal end point in Cartesian space
        T = aubo_FK(self.new_goal)
        self.new_goal_point = Point([T[5][0, 3], T[5][1, 3], T[5][2, 3]])

        # Change the self.stuck_in_local_minimum
        self.stuck_in_local_minimum = True
        print("I get a new goal : ", self.new_goal.vector)

    def check_joint_point_exceed_limit(self, joint_point: Joint_point) -> bool:
        """
        Check the joint_point whether exceed the joint limit angle
        """
        exceed_limit = False

        # 步骤1：检查每个关节是否超出正负180°的范围
        for i in range(len(joint_point.theta)):
            if joint_point.theta[i] < self.joint_limit_angle[0].theta[i] or joint_point.theta[i] > \
                    self.joint_limit_angle[1].theta[i]:
                exceed_limit = True
                break

        # 步骤2：对机器人所在桌面建模，防止关节角碰到桌子
        T = aubo_FK(joint_point)
        link6_point = Point([T[5][0, 3], T[5][1, 3], T[5][2, 3]])
        link5_point = Point([T[4][0, 3], T[4][1, 3], T[4][2, 3]])
        link4_point = Point([T[3][0, 3], T[3][1, 3], T[3][2, 3]])
        link3_point = Point([T[2][0, 3], T[2][1, 3], T[2][2, 3]])
        # link2_point = Point([T[1][0, 3], T[1][1, 3], T[1][2, 3]])

        # print(link6_point.z)
        # print(link5_point.z)
        # print(link4_point.z)
        # print(link3_point.z)
        # print(link2_point.z)
        # print(type(link6_point.z))

        # if link2_point.z < self.limit_position or link3_point.z < self.limit_position or link4_point.z < self.limit_position or link5_point.z < self.limit_position or link6_point.z < self.limit_position:
        if link3_point.z < self.limit_position or link4_point.z < self.limit_position or link5_point.z < self.limit_position or link6_point.z < self.limit_position:
            exceed_limit = True

        return exceed_limit

    # def path_planning(self,joint_output:Queue) -> List[Joint_point]:
    def path_planning(self) -> List[Joint_point]:
        """
        Start the path planning
        """
        # Initial theta
        theta = self.start.theta    # 机器人关节角起点,self.start
        # Initial the path
        path = [self.start]
        while True:
            """
            Initial the U and theta_list
            U : Stores the total potential energy of once search
            joint_point_list : Stores the joint_point of once search
            * 在循环中，计算每个可能的角度组合的能量值（包括吸引力能量和排斥力能量），并将结果存储在列表U中；
            * 根据能量值找到最小值对应的索引，将对应的关节点添加到路径中；
            """
            U = []
            joint_point_list = []

            start = time.time()
            for theta1 in [theta[0] - self.step_length, theta[0], theta[0] + self.step_length]:
                for theta2 in [theta[1] - self.step_length, theta[1], theta[1] + self.step_length]:
                    for theta3 in [theta[2] - self.step_length, theta[2], theta[2] + self.step_length]:
                        for theta4 in [theta[3] - self.step_length, theta[3], theta[3] + self.step_length]:
                            for theta5 in [theta[4] - self.step_length, theta[4], theta[4] + self.step_length]:
                                theta6 = self.goal.theta[5]    # 末端关节角直接对齐goal，保证后面计算可靠性
                                joint_point = Joint_point([theta1, theta2, theta3, theta4, theta5, theta6])
                                # Check collision
                                if auboi5_collision_check(joint_point,
                                                          self.get_obstacle()) or self.check_joint_point_exceed_limit(
                                    joint_point):
                                    # print("关节角不符合要求")
                                    pass
                                else:
                                    # Get the atrractive energy
                                    Uatt = self.get_attractive_energy(joint_point)
                                    # Get the repulsive energy
                                    Urep = self.get_repulsive_energy(joint_point)
                                    U.append(Uatt + Urep)  # The total potential energy
                                    joint_point_list.append(joint_point)
            end = time.time()
            print("生成一次关节角耗费{:.2f}s".format(end - start))

            # Find the index of minimum U
            index = U.index(min(U))
            # Add the theta_list[index] into path
            path.append(joint_point_list[index])
            # print(path[-1].theta)  # 打印路径path中最后一个元素的theta值；

            # # 加个管道
            # # 加个管道,实时输出关节角
            # while joint_output.empty() is not True:
            #     joint_output.get()
            # joint_output.put(path[-1].theta)

            # Update the theta
            theta = joint_point_list[index].theta
            # Judge whether getting stuck in local minimum
            # 判断是否到达self.goal，是否陷入局部最小值
            if path[-1].theta == path[-2].theta and (not self.stuck_in_local_minimum):
                if lg.norm(path[-1].vector - self.goal.vector) < sqrt((self.step_length / 2) ** 2 * 6):
                    # If path[-1] approximately equal to self.goal, path planning is finished
                    print("APF path planning has done!")
                    break
                else:
                    # Set a signal when getting stuck in local minimum
                    # result: print("I get the new goal"), self.stuck_in_local_minimum = True
                    self.deal_with_local_minimum(start=path[-1],joint_point=path[-1])  # 默认拿最后一个路径点：-1

                    # RRT得到临时目标点self.new_goal后，直接作为path
                    joint_point_list.append(self.new_goal)
                    path.append(self.new_goal)
                    theta = self.new_goal.theta

            # When stuck in local minimum, judge whether meet the self.new_goal
            # 陷入局部最小值后，判断是否到达RRT指定的临时目标点self.new_goal
            if self.stuck_in_local_minimum:
                if lg.norm(self.new_goal.vector - path[-1].vector) < sqrt((self.step_length / 2) ** 2 * 6):
                    print("I have met the new goal!!!!!!!")
                    path = [self.start]
                    self.stuck_in_local_minimum = False
                else:
                    pass
                    # 当向临时目标点规划路径的时候，可能走到一半又陷入了新的极小值点，
                    # 但这个问题比较好解决，思路是再次使用RRT算法再找一个临时目标点

                    # # Get a new attractive joint_point
                    # rrt = RRT(start=joint_point, goal=self.goal, cube_obstacle=self.get_obstacle(),
                    #           joint_limit_angle=self.joint_limit_angle, num_iterations=100)
                    # self.new_goal = rrt.path_planning()[-1]  # 返回path路径点，只取最后一个作为新的起始点
                    #
                    # # Get the new goal end point in Cartesian space
                    # T = aubo_FK(self.new_goal)
                    # self.new_goal_point = Point([T[5][0, 3], T[5][1, 3], T[5][2, 3]])
                    #
                    # # Change the self.stuck_in_local_minimum
                    # self.stuck_in_local_minimum = True
                    # print("I get the new goal : ", self.new_goal.vector)

                    # self.deal_with_local_minimum(path[-1])

        # # 加个管道,实时输出关节角
        # joint_output.put(path)
        return path


if __name__ == '__main__':
    start = Joint_point([0] * 6)
    goal = Joint_point([pi / 2] + [0] * 5)
    cube_obstacle = Cube_obstacle(Point([-0.6, -0.6, 0.15]), 0.3, 0.3, 0.3)
    joint_limit_angle = [
        Joint_point(
            [-170 * pi / 180, -90 * pi / 180, -170 * pi / 180, -170 * pi / 180, -170 * pi / 180, -170 * pi / 180]),
        Joint_point([170 * pi / 180, 90 * pi / 180, 170 * pi / 180, 170 * pi / 180, 170 * pi / 180, 170 * pi / 180])]
    skeleton_position_list = [-0.23718073964118958, -0.6300328969955444, 0.9859377145767212, -0.18682724237442017,
                              -0.3185788094997406, 0.9825239181518555, -0.0958828404545784, 0.12313123047351837,
                              0.9762865304946899, -0.2854323983192444, -0.10520001500844955, 0.9101864099502563,
                              -0.45585182309150696, -0.1852768212556839, 0.8236212730407715, -0.5312258005142212,
                              -0.06059045344591141, 0.7187016010284424, 0.018473796546459198, -0.12311580777168274,
                              1.0096009969711304, 0.2087603062391281, -0.20838142931461334, 0.9734570384025574,
                              0.2926161587238312, -0.028986740857362747, 0.9345725178718567, -0.1479041874408722,
                              -0.08855410665273666, 0.9674540162086487]
    joint_position_list = [0.0, -0.0, 0.0985, 0.09015006467853738, -0.0814568342034941, 0.09850000000000002,
                           0.13105307436421948, -0.03618857642294668, 0.5019126034671784, 0.3715983664018288,
                           0.23002817318868934, 0.38947326557888984, 0.4235396556924686, 0.287512737066289,
                           0.45658419898491714, 0.48124881149778836, 0.3155648722781763, 0.38789117862410233]
    # apf_rrt = APF_RRT(start, goal, cube_obstacle, joint_limit_angle)
    # path = apf_rrt.path_planning()
    # print("path", path)
    # apf_rrt.get_obstacle(skeleton_position_list,joint_position_list)
    cube_obstacle: Cube_obstacle = get_obstacle(skeleton_position_list, joint_position_list)
    print("obstacle positiopn(xyz,lwh):", cube_obstacle.x, cube_obstacle.y, cube_obstacle.z, cube_obstacle.l,
          cube_obstacle.w, cube_obstacle.h)
    
